#include <devices_ext/cmt.h>
#include <stdlib.h>
#include <stdarg.h>
#include <leaf/base.h>
#include <leaf/vector.h>
#include <devices/uart.h>

#define cmt_read( cp, buf, sz ) uart_read( cp, buf, sz )
#define cmt_write( cp, buf, sz ) uart_write( cp, buf, sz )
/*初始化Cmt对象*/
void cmt_init(Cmt *cmt, CmtPort port, CmtId id){
	cmt->port = port;
	cmt->id   = id;
}

/*计算校验和（消息中除了0xFA之外的部分通能加起来，如果低字节为0则是正确的消息）*/
uint8_t computeChecksum(const CmtMsg *msg){
	uint8_t cs = 0;
	if(msg->size < 255){
		cs += (uint8_t)msg->size;
	}
	else{
		cs += 0xff;
		cs += (uint8_t)(msg->size>>8);
		cs += (uint8_t)msg->size;
	}
	cs += msg->mid;
	int i;
	for(i = 0; i < msg->size; ++i){
		cs += (uint8_t)vectorGet(&msg->data, i);
	}
	return cs;
}

/*计算校验位*/
uint8_t computeCheckbyte(const CmtMsg *msg)
{
	uint8_t cs = 0;
	if(msg->size < 255){
		cs -= (uint8_t)msg->size;
	}
	else{
		cs -= 0xff;
		cs -= (uint8_t)(msg->size>>8);
		cs -= (uint8_t)msg->size;
	}
	cs -= msg->mid;
	size_t i;
	for(i = 0; i < msg->size; ++i){
		cs -= (uint8_t)vectorGet(&msg->data, i);
	}
	return cs;
}

/*接收一条消息，返回消息的ID，如果返回-1表示校验失败，收到错误消息*/
int cmt_recv(Cmt *cmt, CmtMsg *msg){
	char preamble;
	CmtId id;
	uint8_t checksum;
	cmtmsg_reset(msg);
	do{
		do{
			cmt_read(cmt->port, &preamble, 1);
		}while(preamble != 0xfa);   //等待消息头
		cmt_read(cmt->port, &id, 1);
	}while( id != cmt->id ); //等待正确的node id
	cmt_read(cmt->port, &msg->mid, 1);
	msg->size = 0;
	void *sz  = &msg->size;
	cmt_read(cmt->port, sz, 1);
	if(msg->size == 255){
		cmt_read(cmt->port, sz + 1, 1);
		cmt_read(cmt->port, sz,     1);
	}
	vectorExtends(&msg->data, msg->size);
	int rc = cmt_read(cmt->port, vectorGetBuffer(&msg->data, 0), msg->size);
	if( rc != msg->size ){
		return -1;
	}
	cmt_read(cmt->port, &checksum, 1);
	if( (uint8_t)(computeChecksum(msg) + checksum + cmt->id) )
	{
		return -2;
	}
	return msg->mid;
}

/*发送一条消息，附加校验字节*/
void cmt_send(Cmt *cmt, CmtMsg *msg){
	uint8_t msgHeader[6];
	msgHeader[0] = 0xfa;
	msgHeader[1] = cmt->id;
	msgHeader[2] = msg->mid;
	if(msg->size < 255){
		msgHeader[3] = (uint8_t)msg->size;
		cmt_write(cmt->port, msgHeader, 4);
	}
	else{
		msgHeader[3] = 255;
		msgHeader[4] = (uint8_t)(msg->size >> 8);
		msgHeader[5] = (uint8_t)(msg->size);
		cmt_write(cmt->port, msgHeader, 6);
	}
	cmt_write(cmt->port, vectorGetBuffer(&msg->data, 0), msg->size);
	uint8_t cs = computeCheckbyte(msg) - cmt->id;
	cmt_write(cmt->port, &cs, 1);
}

/* 快捷的消息发送方式，占用较多栈空间
 * 传入send指针为NULL的话，动态分配内存空间，创建新的消息体
 * len是数据长度，若不为0，后面应加上要发送的数据
 * e.g: cmtRequest(cmt, msg, 48, 0);
 *      cmtRequest(cmt, NULL, 48, 0);
 *      cmtRequest(cmt, NULL, 208, 2, 0, 4	);
 *      cmtRequest(cmt, msg, 208, 2, 0, 4	);
 * */
void cmt_request(Cmt *cmt, CmtMsg *send, CmtMsgId mid,size_t len, ...){
	Bool createMsg = PARSE_BOOL(send == NULL);
	if(createMsg){
		send = (CmtMsg *)malloc(sizeof(CmtMsg));
		cmtmsg_init(send, len);
	}
	cmtmsg_reset(send);
	cmtmsg_setid(send, mid);
	if(len > 0){
		va_list params;
		va_start(params, len);
		while(len-- > 0){
			cmtmsg_append_byte(send, va_arg( params, int ));
		}
		va_end(params);
	}
	cmt_send(cmt, send);
	if(createMsg){
		cmtmsg_free(send);
		free(send);
	}
}


void cmt_reset( Cmt *cmt, CmtResetType type)
{
	CmtMsg msg;
	cmtmsg_init( &msg, 3 );
	cmt_request( cmt, &msg, CmtMidReset, 2, 0, type );
	cmtmsg_free( &msg );
}

